#ifndef ROUNDEDSTAR_H
#define ROUNDEDSTAR_H

#include <algorithm>
#include "Core/Vec.h"
#include "Core/numlib.h"

// Represent the level set of
// alpha + beta * cos(omega * theta) - r
// in the (r,theta) plane.
class RoundedStar
{
public:
  enum { Dim = 2 };
  using rVec = Vec<Real, Dim>;

  RoundedStar(Real avrRadius, Real flunct, int period)
      : alpha(avrRadius), beta(flunct), omega(period)
  {
    stablizers = new rVec[2*omega];
    dist = new Real[2*omega];
    auto _r = [&](Real theta) { return alpha + beta * cos(omega*theta); };
    for(int i = 0; i < 2*omega; ++i) {
      Real t = (i + 1.0/2) * M_PI / omega;
      Real rt = _r(t);
      stablizers[i] = { rt * cos(t), rt * sin(t) };
    }
  }

  ~RoundedStar() {
    delete dist;
    delete stablizers;
  }

  rVec getNormalAt(Real r, Real theta) const {
    rVec a { cos(theta), sin(theta) };
    rVec b { -sin(theta), cos(theta) };
    rVec n =  a + b * (beta*omega*sin(omega*theta)/r);
    return normalize(n);
  }

  void check(rVec x, rVec &closestPoint, rVec &normal, bool &contained, Real tol) const
  {
    // 0. Determine the relative position.
    const Real r = norm(x, 2);
    const Real theta = atan2(x[1], x[0]);
    auto _r = [&](Real theta) { return alpha + beta * cos(omega*theta); };
    auto _dr = [&](Real theta) { return -beta * omega * sin(omega*theta); };
    auto _ddr = [&](Real theta) { return -beta * ipow<2>(omega) * cos(omega*theta); };
    contained = r < _r(theta);
    // 1. Find an initial guess.
    for(int i = 0; i < 2*omega; ++i)
      dist[i] = norm(x - stablizers[i], 2);
    int i0 = std::min_element(&dist[0], &dist[2*omega]) - &dist[0];
    Real theta0 = (i0 + 1.0/2) * M_PI / omega;
    // 2. Use the Newton iteration to find the minimizer.
    // let L be the distance between x and (_r(t), t)
    auto dL = [&](Real theta) {
      Real ct = cos(theta), st = sin(theta);
      Real r = _r(theta), dr = _dr(theta);
      Real a = r*ct, b = r*st;
      Real u = (a-x[0]) * (dr*ct - b);
      Real v = (b-x[1]) * (dr*st + a);
      return (u+v) * 2;
    };
    auto ddL = [&](Real theta) {
      Real ct = cos(theta), st = sin(theta);
      Real r = _r(theta), dr = _dr(theta), ddr = _ddr(theta);
      Real a = r*ct, b = r*st;
      Real u = ipow<2>(dr*ct - b) + (a-x[0]) * (ddr*ct - 2*dr*st - a);
      Real v = ipow<2>(dr*st + a) + (b-x[1]) * (ddr*st + 2*dr*ct - b);
      return (u+v) * 2;
    };
    Real t = theta0;
    backtrack(dL, ddL, t, M_PI / omega / 10, M_PI / omega / 10, 20);
    t = fzero(dL, ddL, t, 3, tol);
    // 3. Derive other information.
    Real rt = _r(t);
    closestPoint[0] = rt * cos(t);
    closestPoint[1] = rt * sin(t);
    normal = getNormalAt(rt, t);
    rVec v = x - closestPoint;
    Real delta = std::abs(dot(normal, v) / norm(v, 2));
    if(delta < 1 - 1e-3 || get_dbglevel() >= 2)
      dbgcout1 << "RoundedCorner::check() outputs " << std::scientific << std::setprecision(3)
               <<  delta << std::defaultfloat << "\n";
  }

protected:
  Real alpha;
  Real beta;
  int omega;
  rVec *stablizers;
  Real *dist;
};

#endif // ROUNDEDSTAR_H
